﻿using System;
using System.Collections.Generic;

namespace GeoFits
{    
    /// <summary>
    /// 基础类：四元数
    /// </summary>
    /// <remarks>
    /// 四元数类：主要实现的四元数的基础运算和旋转的相关功能
    /// </remarks>
    public class Quaternion
    {
        #region 基本函数和属性    
        /// <summary>
        /// 四元数的实部w
        /// </summary>
        /// <value>四元数的实部w</value>
        public double w
        { get; private set; }
        /// <summary>
        /// 四元数的虚部x
        /// </summary>
        /// <value>四元数的虚部x</value>
        public double x
        { get; private set; }
        /// <summary>
        /// 四元数的虚部y
        /// </summary>
        /// <value>四元数的虚部y</value>
        public double y
        { get; private set; }
        /// <summary>
        /// 四元数的虚部z
        /// </summary>
        /// <value>四元数的虚部z</value>
        public double z
        { get; private set; }
        /// <summary>
        /// 构造一个四元数
        /// </summary>
        /// <param name="w">四元数的实部w</param>
        /// <param name="x">四元数的虚部x</param>
        /// <param name="y">四元数的虚部y</param>
        /// <param name="z">四元数的虚部z</param>
        public Quaternion(double w, double x, double y, double z)
        { this.w = w; this.x = x; this.y = y; this.z = z; }
        /// <summary>
        /// 用矢量构造一个四元数
        /// </summary>
        /// <param name="v">矢量</param>
        public Quaternion(Vector v)
        { w = 0; x = v.X; y = v.Y; z = v.Z; }
        /// <summary>
        /// 四元数的长度
        /// </summary>
        /// <value>四元数的长度</value>
        public double Length
        {
            get
            {
                return Math.Sqrt(w * w + x * x + y * y + z * z);
            }
        }
        #endregion
        #region 运算符重载
        /// <summary>
        /// 四元数乘法
        /// </summary>
        /// <param name="op1">乘前变量</param>
        /// <param name="rq">乘后变量</param>
        /// <returns>结果：积</returns>
        public static Quaternion operator *(Quaternion op1, Quaternion rq)
        {
            // the constructor takes its arguments as (w, x, y, z)
            return new Quaternion(
                             op1.w * rq.w - op1.x * rq.x - op1.y * rq.y - op1.z * rq.z,
                             op1.w * rq.x + op1.x * rq.w + op1.y * rq.z - op1.z * rq.y,
                             op1.w * rq.y + op1.y * rq.w - op1.x * rq.z + op1.z * rq.x,
                             op1.w * rq.z + op1.z * rq.w + op1.x * rq.y - op1.y * rq.x);
        }
        /// <summary>
        /// 四元数与向量乘法
        /// 表示用四元数对向量进行旋转
        /// 即使四元数的长度不为1，也不会对向量进行缩放
        /// </summary>
        /// <param name="vt">被旋转的向量</param>
        /// <param name="qt">四元数</param>
        /// <returns>结果：积</returns>
        public static Vector operator *(Vector vt, Quaternion qt)
        {
            Quaternion retQt = qt * new Quaternion(vt) * qt.Inverse();
            // the constructor takes its arguments as (w, x, y, z)
            return new Vector(retQt.x, retQt.y, retQt.z);
        }
        /// <summary>
        /// 四元数与向量乘法
        /// 表示用四元数对向量进行旋转
        /// 即使四元数的长度不为1，也不会对向量进行缩放
        /// </summary>
        /// <param name="qt">四元数</param>
        /// <param name="vt">被旋转的向量</param>
        /// <returns>结果：积</returns>
        public static Vector operator *(Quaternion qt, Vector vt)
        {
            return vt * qt;
        }
        /// <summary>
        /// 四元数与点乘法
        /// 此方法是为了方便表示而建立的，此方法实际上将点看作一个0为起点的向量
        /// 表示用四元数对点进行旋转 
        /// </summary>
        /// <param name="pt">被旋转的点</param>
        /// <param name="qt">四元数</param>
        /// <returns>结果：积</returns>
        public static Point operator *(Point pt, Quaternion qt)
        {
            Vector vt = new Vector(pt.X, pt.Y, pt.Z) * qt;
            return new Point(vt.X, vt.Y, vt.Z);
        }
        /// <summary>
        /// 四元数与点乘法
        /// 此方法是为了方便表示而建立的，此方法实际上将点看作一个0为起点的向量
        /// 表示用四元数对点进行旋转 
        /// </summary>
        /// <param name="pt">被旋转的点</param>
        /// <param name="qt">四元数</param>
        /// <returns>结果：积</returns>
        public static Point operator *(Quaternion qt, Point pt)
        {
            return pt * qt;
        }
        /// <summary>
        /// 四元数加法
        /// </summary>
        /// <param name="v">加数</param>
        /// <param name="qt">被加数</param>
        /// <returns>和</returns>
        public static Quaternion operator +(Quaternion v, Quaternion qt)
        {
            return new Quaternion(qt.w + v.w, qt.x + v.x, qt.y + v.y, qt.z + v.z);
        }
        #endregion

        #region 静态函数        
        
        /// <summary>
        /// 使用向量和角度点进行旋转
        /// 角度按照向量的右手螺旋方向为正
        /// </summary>
        /// <param name="pt">被旋转的点</param>
        /// <param name="v">旋转的轴向量</param>
        /// <param name="angle">旋转过的角度</param>
        /// <returns>旋转后所得的点</returns>
        public static Point Rotation(Point pt, Vector v, double angle)
        {
            return pt * getQuaternion(v, angle);
        }
        /// <summary>
        /// 使用向量和角度点进行旋转
        /// 角度按照向量的右手螺旋方向为正
        /// </summary>
        /// <param name="ptList">被旋转的点串</param>
        /// <param name="v">旋转的轴向量</param>
        /// <param name="angle">旋转过的角度</param>
        /// <returns>旋转后所得的点串</returns>
        public static List<Point> Rotation(List<Point> ptList, Vector v, double angle)
        {
            List<Point> ptListAftRot = new List<Point>();
            foreach(var pt in ptList)
            {
                ptListAftRot.Add(Rotation(pt, v, angle));
            }
            return ptListAftRot;
        }
        /// <summary>
        /// 使用四元数对点进行旋转
        /// </summary>
        /// <param name="pt">要被旋转的点</param>
        /// <param name="qt">旋转对应的四元数</param>
        /// <returns>旋转后的点</returns>
        public static Point Rotation(Point pt, Quaternion qt)
        {
            return pt * qt;
        }
        /// <summary>
        /// 使用四元数对点进行旋转
        /// </summary>
        /// <param name="ptList">要被旋转的点串</param>
        /// <param name="qt">旋转对应的四元数</param>
        /// <returns>旋转后的点串</returns>
        public static List<Point> Rotation(List<Point> ptList, Quaternion qt)
        {
            List<Point> ptListAftRot = new List<Point>();
            foreach (var pt in ptList)
            {
                ptListAftRot.Add(Rotation(pt, qt));
            }
            return ptListAftRot;
        }

        /// <summary>
        /// 使用向量和角度向量进行旋转
        /// 角度按照向量的右手螺旋方向为正
        /// </summary>
        /// <param name="vt">被旋转的向量</param>
        /// <param name="v">旋转的轴向量</param>
        /// <param name="angle">旋转过的角度</param>
        /// <returns>旋转后所得的向量</returns>
        public static Vector Rotation(Vector vt, Vector v, double angle)
        {
            return vt * getQuaternion(v, angle);
        }

        private static Quaternion getQuaternion(Vector v, double angle)
        {
            Quaternion u = new Quaternion(v.Normalize() * Math.Sin(angle / 2));
            u.w = Math.Cos(angle / 2);
            return u;
        }

        /// <summary>
        /// 使用向量和角度对向量进行旋转
        /// 角度按照向量的右手螺旋方向为正
        /// </summary>
        /// <param name="vtList">被旋转的向量序列</param>
        /// <param name="v">旋转的轴向量</param>
        /// <param name="angle">旋转过的角度</param>
        /// <returns>旋转后所得的向量序列</returns>
        public static List<Vector> Rotation(List<Vector> vtList, Vector v, double angle)
        {
            List<Vector> ptListAftRot = new List<Vector>();
            foreach (var vt in vtList)
            {
                ptListAftRot.Add(Rotation(vt, v, angle));
            }
            return ptListAftRot;
        }
        /// <summary>
        /// 使用四元数对向量进行旋转
        /// </summary>
        /// <param name="vt">要被旋转的向量</param>
        /// <param name="qt">旋转对应的四元数</param>
        /// <returns>旋转后的向量</returns>
        public static Vector Rotation(Vector vt, Quaternion qt)
        {
            return vt * qt;
        }
        /// <summary>
        /// 使用四元数对点进行旋转
        /// </summary>
        /// <param name="vtList">要被旋转的点串</param>
        /// <param name="qt">旋转对应的四元数</param>
        /// <returns>旋转后的点串</returns>
        public static List<Vector> Rotation(List<Vector> vtList, Quaternion qt)
        {
            List<Vector> ptListAftRot = new List<Vector>();
            foreach (var vt in vtList)
            {
                ptListAftRot.Add(Rotation(vt, qt));
            }
            return ptListAftRot;
        }
        
        
        /// <summary>
        /// 四元数的共轭运算
        /// </summary>
        /// <param name="qt">四元数</param>
        /// <returns>四元数的共轭数</returns>
        public static Quaternion Conjugate(Quaternion qt)
        {
            return new Quaternion(qt.w, -qt.x, -qt.y, -qt.z);
        }
        /// <summary>
        /// 四元数的逆运算
        /// </summary>
        /// <param name="qt">四元数</param>
        /// <returns>四元数的逆</returns>
        public static Quaternion Inverse(Quaternion qt)
        {
            double lengthP2 = Math.Pow(qt.Length, 2);
            return new Quaternion(qt.w / lengthP2, -qt.x / lengthP2, -qt.y / lengthP2, -qt.z / lengthP2);
        }


        #endregion
        /// <summary>
        /// 静态函数：由旋转矩阵获取四元数
        /// </summary>
        /// <param name="rotationMatrix">旋转矩阵</param>
        /// <returns>四元数</returns>
        public static Quaternion getQuaternion(Matrix rotationMatrix)
        {
            //可以处理的矩阵有两种，一种是3*3的，为旋转矩阵
            int n = rotationMatrix.NoCols;
            if (n != rotationMatrix.NoRows)
            {
                throw new MatrixNotSquare();
            }
            else if (n != 3)
            {
                throw new MatrixDimensionException();
            }
            if (Matrix.Det(rotationMatrix) != 1)
            {
                //旋转矩阵的行列式一定为1，不为一表明不是旋转矩阵
                throw new MatrixLibraryExceptions("此矩阵不为旋转矩阵！");
            }

            double wPower = rotationMatrix[0, 0] + rotationMatrix[1, 1] + rotationMatrix[2, 2] + 1;
            double xPower = rotationMatrix[0, 0] - rotationMatrix[1, 1] - rotationMatrix[2, 2] + 1;
            double yPower = -rotationMatrix[0, 0] + rotationMatrix[1, 1] - rotationMatrix[2, 2] + 1;
            double zPower = -rotationMatrix[0, 0] - rotationMatrix[1, 1] + rotationMatrix[2, 2] + 1;
            double w, x, y, z;
            if (wPower > xPower && wPower > yPower && wPower > zPower)
            {
                w = Math.Sqrt(wPower) / 2;
                x = -(rotationMatrix[1, 2] - rotationMatrix[2, 1]) / 4 / w;
                y = -(rotationMatrix[2, 0] - rotationMatrix[0, 2]) / 4 / w;
                z = -(rotationMatrix[0, 1] - rotationMatrix[1, 0]) / 4 / w;
            }
            else if (xPower > wPower && xPower > yPower && xPower > zPower)
            {
                x = Math.Sqrt(xPower) / 2;
                w = -(rotationMatrix[1, 2] - rotationMatrix[2, 1]) / 4 / x;
                y = (rotationMatrix[1, 0] + rotationMatrix[0, 1]) / 4 / x;
                z = (rotationMatrix[2, 0] + rotationMatrix[0, 2]) / 4 / x;
            }
            else if (yPower > wPower && yPower > xPower && yPower > zPower)
            {
                y = Math.Sqrt(yPower) / 2;
                w = -(rotationMatrix[2, 0] - rotationMatrix[0, 2]) / 4 / y;
                x = (rotationMatrix[0, 1] + rotationMatrix[1, 0]) / 4 / y;
                z = (rotationMatrix[1, 2] + rotationMatrix[2, 1]) / 4 / y;
            }
            else
            {
                z = Math.Sqrt(zPower) / 2;
                w = -(rotationMatrix[0, 1] - rotationMatrix[1, 0]) / 4 / z;
                x = (rotationMatrix[2, 0] + rotationMatrix[0, 2]) / 4 / z;
                y = (rotationMatrix[1, 2] + rotationMatrix[2, 1]) / 4 / z;
            }
            return new Quaternion(w, x, y, z);
        }
        /// <summary>
        /// 获取与四元数对应的旋转矩阵
        /// </summary>
        /// <param name="qt">四元数</param>
        /// <returns>旋转矩阵</returns>
        public static Matrix getRotationMatrix(Quaternion qt)
        {
            Quaternion unitQt = qt.Normalize();
            Matrix mat = new Matrix(3, 3);

            mat[0, 0] = 1 - 2 * unitQt.y * unitQt.y - 2 * unitQt.z * unitQt.z;
            mat[1, 1] = 1 - 2 * unitQt.x * unitQt.x - 2 * unitQt.z * unitQt.z;
            mat[2, 2] = 1 - 2 * unitQt.y * unitQt.y - 2 * unitQt.x * unitQt.x;
            mat[0, 1] = 2 * unitQt.x * unitQt.y - 2 * unitQt.w * unitQt.z;
            mat[1, 0] = 2 * unitQt.x * unitQt.y + 2 * unitQt.w * unitQt.z;
            mat[0, 2] = 2 * unitQt.x * unitQt.z + 2 * unitQt.w * unitQt.y;
            mat[2, 0] = 2 * unitQt.x * unitQt.z - 2 * unitQt.w * unitQt.y;
            mat[1, 2] = 2 * unitQt.y * unitQt.z - 2 * unitQt.w * unitQt.x;
            mat[2, 1] = 2 * unitQt.y * unitQt.z + 2 * unitQt.w * unitQt.x;
            return mat;
        }

        /// <summary>
        /// 解出从一个向量旋转到另外一个向量所对应的四元数
        /// </summary>
        /// <param name="vFrom">起始向量</param>
        /// <param name="vTo">目标向量</param>
        /// <returns>旋转对应的四元数</returns>
        public static Quaternion getQuaternion(Vector vFrom, Vector vTo)
        {
            Quaternion qt = (new Quaternion(vTo.Normalize())) * (new Quaternion(vFrom.Normalize())).Inverse();
            double angle = Math.Acos(qt.w) / 2;
            Vector dir = getRotationAxis(qt);
            Quaternion retQt = new Quaternion(dir * Math.Sin(angle));
            retQt.w = Math.Cos(angle);
            return retQt;
        }
        /// <summary>
        /// 获取与四元数相关的旋转轴（已经标准化）
        /// </summary>
        /// <param name="qt">四元数</param>
        /// <returns>标准化的旋转轴</returns>
        public static Vector getRotationAxis(Quaternion qt)
        {
            return new Vector(qt.x, qt.y, qt.z).Normalize();
        }
        /// <summary>
        /// 获取与四元数相关的旋转角
        /// </summary>
        /// <param name="qt">四元数</param>
        /// <returns>旋转角</returns>
        public static double getRotationAngle(Quaternion qt)
        {
            return Math.Acos(qt.w / qt.Length) * 2;
        }

        /// <summary>
        /// 将四元数变成单位长度
        /// </summary>
        /// <returns>单位长度的四元数：如果输入四元数为0，则返回0</returns>
        public Quaternion Normalize()
        {
            if (Length != 0)
                return new Quaternion(w / Length, x / Length, y / Length, z / Length);
            else
                return new Quaternion(w, x, y, z);
        }

        /// <summary>
        /// 四元数的共轭运算
        /// </summary>
        /// <returns>四元数的共轭数</returns>
        public Quaternion Conjugate()
        {
            return Conjugate(this);
        }
        /// <summary>
        /// 四元数的逆运算
        /// </summary>
        /// <returns>四元数的逆</returns>
        public Quaternion Inverse()
        {
            return Inverse(this);
        }
        /// <summary>
        /// 使用四元数对点进行旋转
        /// </summary>
        /// <param name="pt">要被旋转的点</param>
        /// <returns>旋转后的点</returns>
        public Point Rotation(Point pt)
        {
            return Rotation(pt, this);
        }
        /// <summary>
        /// 使用四元数对点串进行旋转
        /// </summary>
        /// <param name="ptList">要被旋转的点串</param>
        /// <returns>旋转后的点串</returns>
        public List<Point> Rotation(List<Point> ptList)
        {
            return Rotation(ptList, this);
        }

        /// <summary>
        /// 使用四元数对向量进行旋转
        /// </summary>
        /// <param name="vt">要被旋转的向量</param>
        /// <returns>旋转后的点</returns>
        public Vector Rotation(Vector vt)
        {
            return Rotation(vt, this);
        }
        /// <summary>
        /// 使用四元数对向量串进行旋转
        /// </summary>
        /// <param name="vtList">要被旋转的向量序列</param>
        /// <returns>旋转后的向量序列</returns>
        public List<Vector> Rotation(List<Vector> vtList)
        {
            return Rotation(vtList, this);
        }
        /// <summary>
        /// 获取与四元数对应的旋转矩阵
        /// </summary>
        /// <returns>旋转矩阵</returns>
        public Matrix getRotationMatrix()
        {
            return getRotationMatrix(this);
        }
    }
}
